#!/usr/bin/env python

import roslib; roslib.load_manifest('camera_bounding_interface')

from geometry_msgs.msg import Point
from geometry_msgs.msg import PointStamped
from color_based_tracking.srv import *
from intersector.srv import *
from camera_bounding_interface.srv import *
import rospy

def get_point_on_floor(polygon):
    # the first point in the polygon is the camera
    camera = polygon[0]
    
    # we want the point in the middle of the polygon
    # not including the camera
    # the middle point is (mx, my, mz)
    num_points = float(len(polygon[1:]))
    mx = 0
    my = 0
    mz = 0
    for p in polygon[1:]:
        mx += p.x
        my += p.y
        mz += p.z
    mx /= num_points
    my /= num_points
    mz /= num_points
    
    # find the vector from camera to middle point
    vx = mx - camera.x
    vy = my - camera.y
    vz = mz - camera.z
    
    # find the ratio of the two triangles
    ratio = camera.z / (camera.z - mz)
    
    # scale the small vector
    vx *= ratio
    vy *= ratio
    vz *= ratio
    
    # translate the new vector
    fx = vx + camera.x
    fy = vy + camera.y
    fz = vz + camera.z
    
    # done
    return (fx, fy, fz)

def use_service(req):
	rospy.wait_for_service('find_by_color')
	rospy.wait_for_service('TwoD_2_ThreeD')
	find_by_color = rospy.ServiceProxy('find_by_color', FindByColor)
	convert = rospy.ServiceProxy('TwoD_2_ThreeD', TwoD_2_ThreeD)


	if( req.color not in ['black','blue','gold','gray','green','red','white'] ):
		return FindLocByColorResponse( Point( float('NaN'), float('NaN'), float('NaN') ) , "What is this " + req.color + " you speak of?" )
	else:
		response = find_by_color( FindByColorRequest( req.color ) )
		response = convert( TwoD_2_ThreeDRequest( response.bounding_box ) )
		print response
		ptuple = get_point_on_floor(response.bPyr.polygon.points)
		return FindLocByColorResponse( Point( ptuple[0], ptuple[1], ptuple[2] ) , "" )

if __name__ == '__main__':
	rospy.init_node('find_loc_by_color')
	s = rospy.Service("find_loc_by_color", FindLocByColor, use_service)
	rospy.spin()
